
## frame.shape : [480, 640, 3]

min_speed, max_speed = 0.05, 0.28   #0.28

import time
import cv2

from predict import predicter
predictor = predicter()

from core.robot_connector import RobotConnector
robot = RobotConnector(ip_address='192.168.123.161', port=8000)

from core.PID import PID
# pid_velocity0 = PID(0.1, 0.2,  0.0, 0.01, 0.00, 0.00)  # dt循环时长, max操作变量最大值, min操作变量最小值, Kp比例增益, Kd积分增益, Ki微分增益
# pid_velocity1 = PID(0.1, 0.2, -0.2, 0.10, 0.10, 0.00)
pid_yaw = PID(0.1, 1.2, -1.2, 0.018, 0.01, 0.00)

from core.Camera import Camera
vedio1 = Camera("http://192.168.123.13:5000/video1")
vedio2 = Camera("http://192.168.123.13:5000/video2")

# IpLastSegment = "15"
# udpstrPrevData = "udpsrc address=192.168.123."+ IpLastSegment + " port="
# ## 端口: 前方, 下巴, 左, 右, 腹部
# udpPORT = [9201, 9202, 9203, 9204, 9205]  [1]
# udpstrBehindData = " ! application/x-rtp,media=video,encoding-name=H264 ! rtph264depay ! h264parse ! omxh264dec ! videoconvert ! appsink"
# udpSendIntegratedPipe = udpstrPrevData +  udpPORT + udpstrBehindData
# print('udpSendIntegratedPipe:', udpSendIntegratedPipe)
# vedio2 = Camera(udpSendIntegratedPipe)

def save(img, result, name):
    if result:
        x, y, w, h = result
        img = cv2.rectangle(img, 
                                (x, y), 
                                (x + w, y + h), 
                                (0, 0, 255), 2)
    cv2.imwrite(name+'.jpg', img)
    print('saved')


def get_object(bboxes, object, square=None):
    for box in bboxes:
        if box['type']==object:
            if square and box['w']*box['h']>square:
                return box['x'], box['y'], box['w'], box['h']
    return None


def cross():
    while True:
        frame1 = vedio1.getframe()
        bboxes = predictor.predict(frame1)
        result = get_object(bboxes, 'green_light')
        if result:
            save(frame1, result, 'green')
            break
    for i in range(12):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[max_speed, 0.0], yawSpeed=0.0)
    while True:
        frame2 = vedio2.getframe()
        bboxes = predictor.predict(frame2)
        result = get_object(bboxes, 'blind_path')
        if result:
            break
        else:
            robot.robot_high_control(velocity=[max_speed, 0.0], yawSpeed=0.0)


def obstacle():
    while True:
        frame1 = vedio1.getframe()
        bboxes = predictor.predict(frame1)
        result = get_object(bboxes, 'block', square=250000) #300000
        if result:
            break
        robot.robot_high_control(velocity=[0.12, 0.00], yawSpeed=0.00)
    print('adjustment...')
    for i in range(15):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.00, -0.20], yawSpeed=0.00)
    for i in range(7):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.20, 0.00], yawSpeed=0.00)
    for i in range(5):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.00, 0.00], yawSpeed=0.58)
    for i in range(15):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.2, 0.00], yawSpeed=0.00)
    for i in range(5):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.00, 0.00], yawSpeed=-0.55)


def step():
    for i in range(6):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.20, 0.00], yawSpeed=0.00, footRaiseHeight=0.12)
    for i in range(10):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[max_speed, 0.00], yawSpeed=0.00)
    while True:
        frame2 = vedio2.getframe()
        bboxes = predictor.predict(frame2)
        result = get_object(bboxes, 'blind_path')
        if result:
            break
        else:
            robot.robot_high_control(velocity=[0.20, 0.00], yawSpeed=0.00)


def end():
    for i in range(15):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[0.20, 0.00], yawSpeed=-0.65)




def walk_along():
    items = [0, 0, 0, 0]
    h_frame, w_frame = vedio2.getframe().shape[0:2]
    x_center = 0
    while True:
        frame1 = vedio1.getframe()
        frame2 = vedio2.getframe()
        bboxes1 = predictor.predict(frame1)
        bboxes2 = predictor.predict(frame2)

        if not items[0]:
            # frame_ = frame2[0:int(h_frame/2), :]
            result = get_object(bboxes2, 'crossing')
            if result:
                save(frame2, result, 'cross')
                print('cross...')
                cross()
                print('crossed')
                x_center = w_frame/2
                items[0] = 1
                continue

        if not items[1]:
            result = get_object(bboxes1, 'block', square=10000)
            if result:
                print('obstacle...')
                obstacle()
                print('obstacled')
                x_center = w_frame/2
                items[1] = 1
                continue
            
        if not items[2]:
            result = get_object(bboxes2, 'step')
            if result:
                save(frame2, result, 'step')
                print('step...')
                step()
                print('steped')
                x_center = w_frame/2
                items[2] = 1
                continue
        
        if not items[3] and all(items[:-2]):
            result = get_object(bboxes2, 'red')
            if result:
                save(frame2, result, 'red')
                print('end...')
                end()
                print('END!!!')
                break
        
        result = get_object(bboxes2, 'blind_path')
        if result:
            x, y, w, h = result
            x_center = x + int(w/2)
            y_center = y + int(h/2)

            # color_image = cv2.rectangle(color_image, 
            #                     (x, y), 
            #                     (x + w, y + h), 
            #                     (0, 0, 255), 2)
            # color_image = cv2.circle(color_image, 
            #                     (x_center,y_center), 
            #                     1, 
            #                     (0, 0, 255), 4)
            # color_image = cv2.line(color_image, 
            #                     (int(w_frame/2), 0), 
            #                     (int(w_frame/2), h_frame), 
            #                     (0, 255, 0), 2)


        velocity = [0.0, 0.0]
        yawspeed = 0.0
        
        deviation = abs(w_frame/2 - x_center)
        velocity[0] = max_speed-(max_speed-min_speed)*(deviation/(w_frame/2))
        yawspeed = pid_yaw.calculate(0, x_center - w_frame/2)
        # print('{}, {}'.format(x_center ,yawspeed))

        robot.robot_high_control(velocity=velocity, yawSpeed=yawspeed)


        waitkey = cv2.waitKey(1)
        if waitkey==ord('q'): break

    # cap.release()


def go(n):
    for i in range(n):
        time.sleep(0.2)
        robot.robot_high_control(velocity=[max_speed, 0.0], yawSpeed=0.0)



frame1, frame2 = None, None
print('go...')
go(10)
print('walk...')
walk_along()
frame1, frame2 = None, None

while True:
    time.sleep(0.2)
    robot.robot_high_control(bodyHeight=-1.8)
